/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.templates.commands.TankDriveWithJoystick;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.templates.OI;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author q
 */
public class Chassis extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    RobotDrive drive; //Drive   

    private Jaguar
            LEFT_TRACK = new Jaguar(RobotMap.LEFT_TRACK),
            RIGHT_TRACK = new Jaguar(RobotMap.RIGHT_TRACK);
            
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new TankDriveWithJoystick());
    }
   
    
    public Chassis() {
        drive = new RobotDrive (LEFT_TRACK, RIGHT_TRACK);
        drive.setSafetyEnabled(false);
        
        if(RobotMap.LEFT_TRACK_REVERSED) {
            drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
        }
        
        if(RobotMap.RIGHT_TRACK_REVERSED) {
            drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        }
    }
    
    public void straight() {
        drive.tankDrive(1.0, 1.0);
    }
    
    public void uturn() {
        drive.tankDrive(1.0, -1.0);
    }
    
    public void toTank() {
        setDefaultCommand(new TankDriveWithJoystick());
    }
    public void tankDriveWithJoystick() {
        double left = OI.getInstance().getLeftJoystick().getY();
        double right = OI.getInstance().getRightJoystick().getY();
        drive.tankDrive(left, right);
    }
    
    public void changeMaxDriveSpeed(double change) {
        drive.setMaxOutput(change);
    }
    
    public void updateDash() {
        SmartDashboard.putDouble("Drive Left", LEFT_TRACK.getSpeed());
        SmartDashboard.putDouble("Drive Right", RIGHT_TRACK.getSpeed());
    }
}
